/* 
 *  Incub - incubate your baby robot
 *  Copyright (C) 2008 Stanislaw Szymczyk
 *  e-mail: sszymczy@gmail.com   www: http://cogville.cog-lab.com
 *
 *  This file is part of Incub.
 *
 *  Incub is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  Incub is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with Incub.  If not, see <http://www.gnu.org/licenses/>.
 * 
 */
 
#include "Corba.h"
#include <exception>
#include <iostream>
#include "CorbaHelper.h"
#include "CommunicableSensor.h"
#include "CommunicableMotor.h"

using std::vector;
using std::string;
using std::pair;

CORBA::ORB_ptr Corba::orb;
PortableServer::POA_var Corba::poa;
vector<string> Corba::callbacks;
Gate *Corba::gate;
boost::mutex Corba::mutex;

void Corba::copyJoint(VJointOut *vJoint, Joint* joint) {
	copyEntity(vJoint->entity(), joint->getEntity());
	vector<Motor*>& motors = joint->getMotors();
	for(unsigned i = 0; i < motors.size(); i++) {
		dynamic_cast<GenericCommunicableMotor*>(motors[i])->copyMotor((*vJoint->motors())[i]);
	}
}

void Corba::copyShape(VShapeOut *vShape, Shape* shape) {
}

void Corba::copyEntity(VEntityOut *vEntity, Entity* entity) {
	vector<Shape*>& shapes = entity->getShapes();
	for(unsigned i = 0; i < shapes.size(); i++) copyShape((*vEntity->shapes())[i], shapes[i]);
	vector<Sensor*>& sensors = entity->getSensors();
	for(unsigned i = 0; i < sensors.size(); i++) {
		GenericCommunicableSensor *sensor = dynamic_cast<GenericCommunicableSensor*>(sensors[i]);
		sensor->copySensor((*vEntity->sensors())[i]);
	}
	vector<Joint*>& joints = entity->getJoints();
	for(unsigned i = 0; i != joints.size(); i++) copyJoint((*vEntity->joints())[i], joints[i]);
}

void Corba::copyWorld(VWorldOut *vWorld, World* world) {
	vector<Entity*>& entities = world->getEntities();
	for(unsigned i = 0; i < entities.size(); i++) copyEntity((*vWorld->entities())[i], entities[i]);
}	

pair<VJointOut*, VJointIn*> Corba::activateJoint(Joint* joint) {
	Entity *entity = joint->getEntity();
	pair<VEntityOut*, VEntityIn*> vEntity = activateEntity(entity);
	VJointOut *vJointOut = new VJointOut_i(vEntity.first, new VMotorsOut());
	VJointIn *vJointIn = new VJointIn_i(vEntity.second, new VMotorsIn());
	vector<Motor*>& motors = joint->getMotors();
	vJointOut->motors()->length(motors.size());
	vJointIn->motors()->length(motors.size());
	for(unsigned i = 0; i < motors.size(); i++) {
		Motor *motor = motors[i];
		pair<VMotorOut*, VMotorIn*> vMotor = dynamic_cast<GenericCommunicableMotor*>(motors[i])->activateMotor();
		(*vJointOut->motors())[i] = vMotor.first;
		(*vJointIn->motors())[i] = vMotor.second;
	}
	return std::make_pair(vJointOut, vJointIn);
}

VShapeOut* Corba::activateShape(Shape* shape) {
	VShapeOut *vShape = new VShapeOut_i();
	return vShape;
}

pair<VEntityOut*, VEntityIn*> Corba::activateEntity(Entity* entity) {
	vector<Shape*>& shapes = entity->getShapes();
	vector<Sensor*>& sensors = entity->getSensors();
	vector<Joint*>& joints = entity->getJoints();

	VEntityOut *vEntityOut = new VEntityOut_i(new VJointsOut(), new VShapesOut(), new VSensorsOut());
	vEntityOut->joints()->length(joints.size());
	vEntityOut->shapes()->length(shapes.size());
	vEntityOut->sensors()->length(sensors.size());

	VEntityIn *vEntityIn = new VEntityIn_i(new VJointsIn());
	vEntityIn->joints()->length(joints.size());

	for(unsigned i = 0; i < shapes.size(); i++) {
		(*vEntityOut->shapes())[i] = activateShape(shapes[i]);
	}
	for(unsigned i = 0; i < sensors.size(); i++) {
		GenericCommunicableSensor *sensor = dynamic_cast<GenericCommunicableSensor*>(sensors[i]);
		(*vEntityOut->sensors())[i] = sensor->activateSensor();
	}
	for(unsigned i = 0; i != joints.size(); i++) {
		pair<VJointOut*, VJointIn*> vJoint = activateJoint(joints[i]);
		(*vEntityOut->joints())[i] = vJoint.first;
		(*vEntityIn->joints())[i] = vJoint.second;
	}
	return std::make_pair(vEntityOut, vEntityIn);
}

pair<VWorldOut*, VWorldIn*> Corba::activateWorld(World* world) {
	VWorldOut *vWorldOut = new VWorldOut_i(new VEntitiesOut());
	VWorldIn *vWorldIn = new VWorldIn_i(new VEntitiesIn());
	vector<Entity*>& entities = world->getEntities();
	vWorldOut->entities()->length(entities.size());
	vWorldIn->entities()->length(entities.size());
	for(unsigned i = 0; i < entities.size(); i++) {
		pair<VEntityOut*, VEntityIn*> vEntity = activateEntity(entities[i]);
		(*vWorldOut->entities())[i] = vEntity.first;
		(*vWorldIn->entities())[i] = vEntity.second;
	}
	return std::make_pair(vWorldOut, vWorldIn);
}

void Corba::init(int argc, char **argv, World* world) {
	try {
		orb = CORBA::ORB_init(argc, argv);
		CORBA::ValueFactoryBase_var factory;

		CorbaHelper::registerFactories(orb);

		CORBA::Object_var obj = orb->resolve_initial_references("RootPOA");
		PortableServer::POA_var poa = PortableServer::POA::_narrow(obj);

		pair<VWorldOut*, VWorldIn*> vWorld = activateWorld(world);
		
		gate = new Gate(world, vWorld.first, vWorld.second);

		PortableServer::ObjectId_var gateid = poa->activate_object(gate);

		// Obtain a reference to the object, and register it in the naming service.
		obj = gate->_this();

		bindObjectToName(orb, obj);

		PortableServer::POAManager_var pman = poa->the_POAManager();
		pman->activate();
	} catch(CORBA::SystemException&) {
		throw CorbaException("Caught CORBA::SystemException.");
	} catch(CORBA::Exception&) {
		throw CorbaException("Caught CORBA::Exception.");
	} catch(omniORB::fatalException& fe) {
		throw CorbaException("Caught omniORB::fatalException:");
	}
}

void Corba::cleanup() {
	gate->vWorldOut->_remove_ref();
	gate->vWorldIn->_remove_ref();

	gate->_remove_ref();
	orb->destroy();
}

void Corba::bindObjectToName(CORBA::ORB_ptr orb, CORBA::Object_ptr objref)
{
	CosNaming::NamingContext_var rootContext;

	try {
		// Obtain a reference to the root context of the Name service:
		CORBA::Object_var obj;
		obj = orb->resolve_initial_references("NameService");
		
		// Narrow the reference returned.
		rootContext = CosNaming::NamingContext::_narrow(obj);
		if( CORBA::is_nil(rootContext) ) {
			throw CorbaException("Failed to narrow the root naming context.");
		}
	} catch(CORBA::ORB::InvalidName& ex) {
		// This should not happen!
		throw CorbaException("Service required is invalid [does not exist].");
	}

	try {
		// Bind a context called "test" to the root context:
		
		CosNaming::Name contextName;
		contextName.length(1);
		contextName[0].id   = (const char*) "enaction";   // string copied
		contextName[0].kind = (const char*) "my_context"; // string copied
		
		CosNaming::NamingContext_var testContext;
		try {
			// Bind the context to root.
			testContext = rootContext->bind_new_context(contextName);
		} catch(CosNaming::NamingContext::AlreadyBound& ex) {
			// If the context already exists, this exception will be raised.
			// In this case, just resolve the name and assign testContext
			// to the object returned:
			CORBA::Object_var obj;
			obj = rootContext->resolve(contextName);
			testContext = CosNaming::NamingContext::_narrow(obj);
			if( CORBA::is_nil(testContext) ) {
				throw CorbaException("Failed to narrow naming context.");
			}
		}

		// Bind objref with name Echo to the testContext:
		CosNaming::Name objectName;
		objectName.length(1);
		objectName[0].id   = (const char*) "Gate";   // string copied
		objectName[0].kind = (const char*) "Object"; // string copied
		
		try {
			testContext->bind(objectName, objref);
		}
		catch(CosNaming::NamingContext::AlreadyBound& ex) {
			testContext->rebind(objectName, objref);
		}
		// Note: Using rebind() will overwrite any Object previously bound to /enaction/World with obj.
	} catch(CORBA::COMM_FAILURE& ex) {
		throw CorbaException("Caught system exception COMM_FAILURE -- unable to contact the naming service.");
	} catch(CORBA::SystemException&) {
		throw CorbaException("Caught a CORBA::SystemException while using the naming service.");
	}
}

void Corba::step() {
	copyWorld(gate->vWorldOut, gate->getWorld());
	if (orb->work_pending()) orb->perform_work();
	boost::mutex::scoped_lock scoped_lock(mutex);
	for(vector<string>::iterator objStr = callbacks.begin(); objStr != callbacks.end(); objStr++) {
		CORBA::Object_var obj = orb->string_to_object(objStr->c_str());
		IClientCallback_var callback = IClientCallback::_narrow(obj);
		try {
			callback->call();
		} catch(CORBA::MARSHAL& ex) {
			throw CorbaException((char*) ("Caught MARSHAL exception " + string(ex.NP_minorString())).c_str());
		}
	}
};

void Corba::registerCallback(IClientCallback_ptr call) {
	callbacks.push_back(string(orb->object_to_string(call)));
}

void Corba::unregisterCallback(IClientCallback_ptr call) {
	boost::mutex::scoped_lock scoped_lock(mutex);
	for(vector<string>::iterator objStr = callbacks.begin(); objStr != callbacks.end(); objStr++) {
		string objStr2 = string(orb->object_to_string(call));
		if(*objStr == objStr2) { 
			callbacks.erase(objStr);
			break;
		}
	}
}
